// world class encapsulates the world.
// world::interact() allows you to ineract with the robot/environment.
// Ideally, all communicating with the robot would be hidden inside this object
//  s.t. if a particular processor needs to get/set some value, it will be 
//  available from this object, even if from a romote host.
//
// For now, we can have each node simulate an arm redundantly.


//#include "main.h"
#include <iostream>
#include <iomanip>
#include <fstream>
#include <string>
#include <sstream>
#include <algorithm>
#include <vector>
#include <map>
#include <mpi.h>
#include "motormap.h"

#define APEX2
#ifdef APEX1
  #include "robot_real.h"
#endif

#ifdef APEX2
  #include "robot_sim.h"
#endif   

#ifdef APEX3
  #include "robot_real_fpga.h"
#endif

using namespace std;

extern float x[][3];


class group
{
public:
	string area_name;
	string cell_name;
	int rows;
	int cols;
	int first_neuron_id;
	int last_neuron_id;

	group()
	{
		area_name = "";
		rows = -1;cols=-1;
		first_neuron_id = 1;
		last_neuron_id = 0;
	}
};



class electrode
{
public:
	float position[3];
	int start_msec;
	int stop_msec;
	float dist;
	float stim_current;
	
};


//*******************************************
float dist(float x[3], float y[3], int n=3)
{

	float d = 0.0f;
	for(int i = 0; i < n; i++)
	{
		d+=(x[i]-y[i])*(x[i]-y[i]);
	}
	return sqrt(d);
}

class world 
{
	  
private:
	vector<float> mfr;
	
	vector<electrode> electrodes;
  
public:
	
  world() {
    
	sim_end_time_sec = 100;

  fprintf(stderr, "came all the way here\n");
    // initialization ...
  load_groups("groups.dat");
    // max_neuron_id is set now.

  mfr.resize(max_neuron_id+1,0.0);

    //*************************************
    // Set up the motor control system.
    // Assume that each spot in the layer V pyramidal cell array
    // has a specific muscle synergy that is position based.
    // 

  group g = groups["M1p5(L56)"];

  vector<float> one_row;
  one_row.resize(g.cols,0.0f);

  shoulder_lut_rad.resize(g.rows,one_row);
  deltoid_lut_rad.resize(g.rows,one_row);
  elbow_lut_rad.resize(g.rows,one_row);

  float prow = 0.0f;
  for( int row = 0; row< g.rows; row++)
  {
    float pcol = 0.0f;
    for( int col = 0; col < g.cols; col++ )
    {
      shoulder_lut_rad[row][col] = motorprimitives[(int) prow*25+(int)pcol][0] * range_rad[0];
      deltoid_lut_rad[row][col]  = motorprimitives[(int) prow*25+(int)pcol][1] * range_rad[1];
      elbow_lut_rad[row][col]    = motorprimitives[(int) prow*25+(int)pcol][2] * range_rad[2];		
      pcol=pcol+24.999/g.cols;
    }
    prow=prow+24.999/g.rows;
  }


};

~world() 
{
  cout << "Quitting now. Goodbye.\n";

  if( myprocid == 0 )
  {
      //apex.quit();
    fout.close();
    patternhist.close();
  }

};

void init_robot(int myid)
{

  myprocid = myid;

  //apex.initializer();
  apex.init(myid);
  
  if( myprocid == 0 )fout.open("motorcommands.dat");
  if( myprocid == 0 )patternhist.open("patternhist.dat");

    // Debug:
  if( myprocid==0)
  {
    group g = groups["M1p5(L56)"];

    for( int row = 0; row< g.rows; row++)
    {
      for( int col = 0; col < g.cols; col++ )
      {
        cout << "(" << shoulder_lut_rad[row][col]<< "," << deltoid_lut_rad[row][col]<< "," << elbow_lut_rad[row][col]<<") ";
      }
      cout << endl<< endl;
    }
  }

};

int myrand(int low, int hi)
{
  return low + rand()%(hi-low);	
}


float to01(float x,float minx,float maxx)
{
  return (x- minx)/(maxx-minx);
}

  // Returns muscle length as a function of joint angle in radians, given the 
  // length of the tendon insertion points from the joint.
float length(float a, float b, float theta_rad)
{
  return sqrt(a*a+b*b-2*a*b*cos(theta_rad));
}
	
// Returns derivative of the muscle length with respect to the joint angle (radians),
// given the length of the tendon insertion points from the joint.
// This can be used to convert angular joint velocity in radians per second to muscle 
// length changes in linear units by calculating dtheta*dlength(theta).
float dlength(float a, float b, float theta_rad)
{
		return a*b*sin(theta_rad)/sqrt(a*a+b*b-2*a*b*cos(theta_rad));
}	
	

//***************************************************************************************************
void stim_group(string group_name, vector<electrode> electrodes, int t_msec, int trial_duration_msec, float exc_output[])
{		
  group g;
  if( groups.find(group_name)!=groups.end() )
  {
    g = groups[group_name];
    int gnum=g.last_neuron_id-g.first_neuron_id;

    int i = g.first_neuron_id;
    for( int row = 0; row< g.rows; row++)
    {
      for( int col = 0; col < g.cols; col++ )
      {
        for( int elect=0; elect < electrodes.size(); elect ++ )
        {
          if( t_msec%trial_duration_msec >= electrodes[elect].start_msec && t_msec%trial_duration_msec <= electrodes[elect].stop_msec){
            if( dist(x[i], electrodes[elect].position) < electrodes[elect].dist )
            {
              exc_output[i] = electrodes[elect].stim_current;
            }
          }			
        }							
        i++;
      }
    }
  }
}	
	
	
//*******************************	
// Get the spiked array so we can keep track of spikes. 
// Get a pointer to the external current array array so we can stimulate neurons.
// 	set exc_output[neuron] to add exc_output[neuron] to a given neuron number.  (Don't worry if the neuron
// 	is on this processor or not; we will take care of that in the simulator.)
// int t in msec.
// 
	
void interact(int t, bool spiked[], float exc_output[] )
{

		static float last_shoulder,last_deltoid,last_elbow; 
		float shoulder,deltoid,elbow, sdot,ddot,edot, storque, dtorque, etorque;
		int sec = t/1000;
		int gnum,pnum;
		
		float n_patterns = 4;
		
		int trial_duration_msec = 2000;
		
		int S1_stimulation_start_time_sec=sim_end_time_sec;
		int S1_stimulation_end_time_sec=sim_end_time_sec+1000000;  // Never stop this; when we restart from checkpoint, keep S1 stimulation.    
		
		int M1_stimulation_start_time_sec=2;
		int M1_stimulation_end_time_sec=sim_end_time_sec-20;   // Next, turn off layer 2/3 stimulation and see if reentry from S1 can activate layer 2.
	
		int M1_out_stimulation_start_time_sec=sim_end_time_sec;
		int M1_out_stimulation_end_time_sec=sim_end_time_sec-40; // First, turn off motor layer stimulation and see if layer 2/3 still activates layer 5.

		// output pattern data
		int pattern_duration_msec = trial_duration_msec/n_patterns;
		int pattern_presentation_time_msec = 250;
		if( t%pattern_duration_msec == 0 )
		{
			patternhist << t << " " << (t%trial_duration_msec) / pattern_duration_msec << endl;
		}
		
		
		float n_electrodes = 8;

		if (sec>=0)
		{	
			apex.get_arm_proprioception(shoulder,deltoid,elbow, sdot,ddot,edot, storque, dtorque, etorque, t);

			// convert to appropriate cell types from paper on spinal cord.
			const float a=1.0,b=.25;  // Muscle tendon insertion points (normalized).
			float temp, temp2;
			group g;

			
			// Try stimulating the intralaminar nucleus to simulate the awake state. McKinstry.  6-15-09.  This caused TCns to become stuck on.  Bummer.
			/*
			if( sec >= 1 ){
				if( groups.find("M1TCn")!=groups.end() ){
					g = groups["M1TCn"];
					for(int gnum=g.first_neuron_id;gnum<g.last_neuron_id;gnum++){
						exc_output[gnum] = 2.0f;
					}
				}
			}
			*/
			
			
			float stim_current = 1500.0;
			float distance = 0.2;
			
			if( sec >= M1_stimulation_start_time_sec && sec <= M1_stimulation_end_time_sec && groups.find("M1TCs")!=groups.end() )
			{

				vector<electrode> electrodes(n_electrodes);
				float center_x = 1.0;
				float center_y = 1.0;

				// Create 4 oriented bars using 3 circles per bar.
				float radius = 0.4;  // DEBUG; was 0.6
				int e = 0;
				for( int i = 0; i < n_patterns; i++ )
				{
					
					float theta_rad = i*2*pi/n_patterns;
					electrodes[e].position[0]=center_x + radius*cos(theta_rad);
					electrodes[e].position[1]=center_y + radius*sin(theta_rad);
					electrodes[e].position[2]=0;
					electrodes[e].start_msec=(int)((float) i*trial_duration_msec/n_patterns); 
					electrodes[e].stop_msec=(int)((float)  i*trial_duration_msec/n_patterns+pattern_presentation_time_msec-1);
					electrodes[e].dist=distance;
					electrodes[e].stim_current = stim_current;
					e++;

					theta_rad = (i+1)*2*pi/n_patterns;
					electrodes[e].position[0]=center_x + radius*cos(theta_rad);
					electrodes[e].position[1]=center_y + radius*sin(theta_rad);
					electrodes[e].position[2]=0;
					electrodes[e].start_msec=(int)((float) i*trial_duration_msec/n_patterns);  
					electrodes[e].stop_msec=(int)((float)  i*trial_duration_msec/n_patterns+pattern_presentation_time_msec-1);
					electrodes[e].dist=distance;
					electrodes[e].stim_current = stim_current;
					e++;
					
				}
				
				
				/*
				// This was used to create single spot stimulation for motor control.
				float radius = 0.8;  // DEBUG; was 0.6
				for( int i = 0; i < n_electrodes; i++ ){
					
					float theta_rad = i*2*pi/n_electrodes;
					electrodes[i].position[0]=center_x + radius*cos(theta_rad);
					electrodes[i].position[1]=center_y + radius*sin(theta_rad);
					electrodes[i].position[2]=0;
					electrodes[i].start_msec=(int)((float) i*trial_duration_msec/n_electrodes+10);  // JLM 7-7-09: trying some pre before post; let S1 start.
					electrodes[i].stop_msec=(int)((float)(i+1)*trial_duration_msec/n_electrodes-1-40);
					electrodes[i].dist=distance;
					electrodes[i].stim_current = stim_current;
					
				}
				*/
				// DEBUG	
				stim_group("M1TCs", electrodes, t, trial_duration_msec, exc_output);
				//stim_group("M1p23", electrodes, t, trial_duration_msec, exc_output);
				
        /*				
				g = groups["M1TCs"];
				gnum=g.last_neuron_id-g.first_neuron_id;
				pnum=(int)(gnum/6);
				
				//cout << g.last_neuron_id << " " << g.first_neuron_id << endl;
				
				
				
				
				
				//for(int i = g.first_neuron_id; i <=g.last_neuron_id; i++){
				temp=(length(a,b,shoulder)-(a-b))/(2*b); // Convert angle to stretch receptor length and normalize to [0-1]
				for (int i = g.first_neuron_id; i <=g.first_neuron_id+pnum/2; i++) {
					exc_output[i]=0.3*temp;
				}
				
				temp=(length(a,b,pi-shoulder)-(a-b))/(2*b); // Convert angle to stretch receptor length and normalize to [0-1]
				for (int i = g.first_neuron_id+pnum/2; i <=g.first_neuron_id+pnum; i++) {
					exc_output[i]=0.3*temp;
				}
				
				// Calculate instantaneous velocity, dmuscle_length/djoint_angle, where joint angle is in radians per sec.
				// a should not equal b!
				temp=dlength(a,b,shoulder);
				temp2 = temp * max(0.0f,sdot);  // dmuscle_length/djoint_angle * djoint_angle/dt = dmuscle_length/dt.
				for (int i=g.first_neuron_id+pnum+1; i<=g.first_neuron_id+pnum+pnum/2;i++)
					exc_output[i]=1.5*temp2;
				
				temp2 = temp * max(0.0f,-sdot);  // dmuscle_length/djoint_angle * djoint_angle/dt = dmuscle_length/dt.
				for (int i=g.first_neuron_id+pnum+pnum/2+1; i<=g.first_neuron_id+2*pnum;i++)
					exc_output[i]=1.5*temp2;
				
				
				// ********* deltoid
				
				temp=(length(a,b,deltoid)-(a-b))/(2*b); // Convert angle to stretch receptor length and normalize to [0-1]
				for (int i=g.first_neuron_id+2*pnum+1; i<=g.first_neuron_id+2*pnum+pnum/2;i++)
					exc_output[i]=0.3*temp;
				
				temp=(length(a,b,pi-deltoid)-(a-b))/(2*b); // Convert angle to stretch receptor length and normalize to [0-1]
				for (int i=g.first_neuron_id+2*pnum+pnum/2+1; i<=g.first_neuron_id+3*pnum;i++)
					exc_output[i]=0.3*temp;
				
				temp=dlength(a,b,deltoid);
				temp2 = temp * max(0.0f,ddot);  // dmuscle_length/djoint_angle * djoint_angle/dt = dmuscle_length/dt.
				for (int i=g.first_neuron_id+3*pnum+1; i<=g.first_neuron_id+3*pnum+pnum/2;i++)
					exc_output[i]=1.5*temp2;
				
				temp2 = temp * max(0.0f,-ddot);  // dmuscle_length/djoint_angle * djoint_angle/dt = dmuscle_length/dt.
				for (int i=g.first_neuron_id+3*pnum+pnum/2+1; i<=g.first_neuron_id+4*pnum;i++)
					exc_output[i]=1.5*temp2;
				
				// *************** elbow
				
				temp=(length(a,b,elbow)-(a-b))/(2*b); // Convert angle to stretch receptor length and normalize to [0-1]
				for (int i=g.first_neuron_id+4*pnum+1; i<=g.first_neuron_id+4*pnum+pnum/2;i++)
					exc_output[i]=0.3*temp;
				
				temp=(length(a,b,pi-elbow)-(a-b))/(2*b); // Convert angle to stretch receptor length and normalize to [0-1]
				for (int i=g.first_neuron_id+4*pnum+pnum/2+1; i<=g.first_neuron_id+5*pnum;i++)
					exc_output[i]=0.3*temp;
				
				temp=dlength(a,b,elbow);
				temp2 = temp * max(0.0f,edot);  // dmuscle_length/djoint_angle * djoint_angle/dt = dmuscle_length/dt.
				for (int i=g.first_neuron_id+5*pnum+1; i<=g.first_neuron_id+5*pnum+pnum/2;i++)
					exc_output[i]=1.5*temp2;
				
				temp2 = temp * max(0.0f,-edot);  // dmuscle_length/djoint_angle * djoint_angle/dt = dmuscle_length/dt.
				for (int i=g.first_neuron_id+5*pnum+pnum/2+1; i<g.first_neuron_id+6*pnum;i++)
					exc_output[i]=1.5*temp2;
*/				
			}
			

			//********************************************************
			if( sec >= S1_stimulation_start_time_sec && sec <= S1_stimulation_end_time_sec && groups.find("S1TCs")!=groups.end() )
			{

				vector<electrode> electrodes(n_electrodes);
				float center_x = 5.0;
				float center_y = 1.0;
				float radius = 0.6;
				for( int i = 0; i < n_electrodes; i++ ){
					
					float theta_rad = i*2*pi/n_electrodes;
					electrodes[i].position[0]=center_x + radius*cos(theta_rad);
					electrodes[i].position[1]=center_y + radius*sin(theta_rad);
					electrodes[i].position[2]=0;
					electrodes[i].start_msec=(int)((float) i*trial_duration_msec/n_electrodes);
					electrodes[i].stop_msec=(int)((float)(i+1)*trial_duration_msec/n_electrodes-1-50);
					electrodes[i].dist=distance;
					electrodes[i].stim_current = stim_current;
					
				}
				
				stim_group("S1p23", electrodes, t, trial_duration_msec, exc_output);
				stim_group("S1p5(L56)", electrodes, t, trial_duration_msec, exc_output);
/*			
				// This representation used for map formation in S1.
				g = groups["S1TCs"];
				int neurons=g.last_neuron_id-g.first_neuron_id;
				const int variables = 3*2*3; // 3 joints * 2 muscles per joint * 3 variables per muscle
				float neurons_per_var= (float)neurons/(float)variables;
				if( neurons_per_var < 1.0 ) {
					cout << "interact(): too few neurons in S1TCs to handle " << variables << " variables."; 
					exit(1);
				}
				
				
				float vars[variables];
				int vn=0;
				vars[vn++]=380*(length(a,b,shoulder)-(a-b))/(2*b); // Convert angle to stretch receptor length and normalize to [0-1]
				vars[vn++]=300*(length(a,b,pi-shoulder)-(a-b))/(2*b); // Convert angle to stretch receptor length and normalize to [0-1]				
				// Calculate instantaneous velocity, dmuscle_length/djoint_angle, where joint angle is in radians per sec.
				// a should not equal b!
				temp=dlength(a,b,shoulder);
				vars[vn++] = 600 * temp * max(0.0f, sdot);  // dmuscle_length/djoint_angle * djoint_angle/dt = dmuscle_length/dt.
				vars[vn++] = 600 * temp * max(0.0f,-sdot);  // dmuscle_length/djoint_angle * djoint_angle/dt = dmuscle_length/dt.
				
				// Muscle force; just using torque as measured by the motors here for simplicity
				vars[vn++] = 600 * max(0.0f, storque);
				vars[vn++] = 600 * max(0.0f, -storque);
				
				// ********* deltoid
				vars[vn++]=380*(length(a,b,deltoid)-(a-b))/(2*b); // Convert angle to stretch receptor length and normalize to [0-1]
				vars[vn++]=300*(length(a,b,pi-deltoid)-(a-b))/(2*b); // Convert angle to stretch receptor length and normalize to [0-1]				
				// Calculate instantaneous velocity, dmuscle_length/djoint_angle, where joint angle is in radians per sec.
				// a should not equal b!
				temp=dlength(a,b,deltoid);
				vars[vn++] = 600 * temp * max(0.0f, ddot);  // dmuscle_length/djoint_angle * djoint_angle/dt = dmuscle_length/dt.
				vars[vn++] = 600 * temp * max(0.0f,-ddot);  // dmuscle_length/djoint_angle * djoint_angle/dt = dmuscle_length/dt.

				// Muscle force; just using torque as measured by the motors here for simplicity
				vars[vn++] = 600 * max(0.0f, dtorque);
				vars[vn++] = 600 * max(0.0f, -dtorque);
				
				
				// *************** elbow
				
				vars[vn++]=380*(length(a,b,elbow)-(a-b))/(2*b); // Convert angle to stretch receptor length and normalize to [0-1]
				vars[vn++]=300*(length(a,b,pi-elbow)-(a-b))/(2*b); // Convert angle to stretch receptor length and normalize to [0-1]				
				// Calculate instantaneous velocity, dmuscle_length/djoint_angle, where joint angle is in radians per sec.
				// a should not equal b!
				temp=dlength(a,b,elbow);
				vars[vn++] = 600 * temp * max(0.0f, edot);  // dmuscle_length/djoint_angle * djoint_angle/dt = dmuscle_length/dt.
				vars[vn++] = 600 * temp * max(0.0f,-edot);  // dmuscle_length/djoint_angle * djoint_angle/dt = dmuscle_length/dt.

				// Muscle force; just using torque as measured by the motors here for simplicity
				vars[vn++] = 600 * max(0.0f, etorque);
				vars[vn++] = 600 * max(0.0f, -etorque);
				
				// Now map the variables to thalamic neurons:
				int id = g.first_neuron_id;
				for( int neuron_offset = 0; neuron_offset < neurons; neuron_offset++){
					exc_output[id+neuron_offset] = vars[ (int) ((float)neuron_offset/neurons_per_var)];
				}
*/
			}
			


			//******************  Motor babbling.  Stimulate the Layer V cells to get it going.
			
			
			if( sec >= M1_out_stimulation_start_time_sec && sec <= M1_out_stimulation_end_time_sec )
			{

				vector<electrode> electrodes(n_electrodes);
				float center_x = 1.0;
				float center_y = 1.0;
				float radius = 0.6;
				for( int i = 0; i < n_electrodes; i++ )
				{
					
					float theta_rad = i*2*pi/n_electrodes;
					electrodes[i].position[0]=center_x + radius*cos(theta_rad);
					electrodes[i].position[1]=center_y + radius*sin(theta_rad);
					electrodes[i].position[2]=0;
					electrodes[i].start_msec=(int)((float) i*trial_duration_msec/n_electrodes-10);
					electrodes[i].stop_msec=(int)((float)(i+1)*trial_duration_msec/n_electrodes-1-40);
					electrodes[i].dist=distance;
					electrodes[i].stim_current = stim_current;
					
				}
				
				
				stim_group("M1p5(L56)", electrodes, t, trial_duration_msec, exc_output);
				//stim_group("M1p5(L23)", electrodes, t, trial_duration_msec, exc_output);
				//stim_group("M1b5", electrodes, t, exc_output);  //Petersen & Sakman ,2001 discussion says that fewer inhib cells likely activated directly by microstim.
				//stim_group("M1nb5", electrodes, t, exc_output);

				
				
			}
			
			
			
			//******************* Part II.  Set the motor commands based on Layer V cell activity.
			// Only procid==0 has to deal with this.  No sense in having everyone do it redundantly.

			
			float shoulder_sensory = shoulder;
			float deltoid_sensory = deltoid; 
			float elbow_sensory = elbow;			

			shoulder = 0.0f;
			deltoid = 0.0f; 
			elbow = 0.0f;			
			
			
			g = groups["M1p5(L56)"];
			gnum=g.last_neuron_id-g.first_neuron_id;
			
			float tau = exp(-1.0/50.0);  // 100 msec time constant; trying to match time constant of slow twitch muscles.
			
			if( myprocid == 0 )
			{
				shoulder = 0.0f;
				deltoid = 0.0f; 
				elbow = 0.0f;
				float rate_sum = 0.0f;
				// 1. Interpret activity
				int i = g.first_neuron_id;
				int votes = 0;
				for( int row = 0; row< g.rows; row++)
				{
					for( int col = 0; col < g.cols; col++ )
					{

						mfr[i]= tau*mfr[i] + (1.0f-tau)*(spiked[i]?1.0:0.0)*1000 ; //This gives mean rate in Hz.
						
						if( mfr[i] > 10.0 )
						{  // only vote if you're serios about it.
							
							// Generate population vector which will determine instantaneous posture target.
							shoulder += mfr[i] * shoulder_lut_rad[row][col];
							deltoid  += mfr[i] * deltoid_lut_rad[row][col];
							elbow    += mfr[i] * elbow_lut_rad[row][col];
							rate_sum += mfr[i];
							votes++;
						}
						
						
						if(t%250 == 100 && myprocid==0) cout <<fixed << setw(5) << setprecision(1)  << mfr[i] << " " ;
						
						
						i++;

					}
					if(t%250 == 100 && myprocid==0) cout <<endl ;
				}
				if(t%250 == 100 && myprocid==0) cout <<endl ;

				
				
/*				
				for(int i = g.first_neuron_id; i <=g.last_neuron_id; i++){
					count_fired += spiked[i] ? 1 : 0;
				}
				
				
				
				if( t%1000 == 999){
					cout << count_fired/(g.rows*g.cols) << endl;
					count_fired=0;
				}
*/				
				
				
				
				if( rate_sum > .00001 && votes >=10)
				{
					shoulder *= 1.0f/rate_sum;
					deltoid *= 1.0f/rate_sum;
					elbow *= 1.0f/rate_sum;			
				}
				else
				{
					shoulder = home_pose_rad[0];
					deltoid = home_pose_rad[1];
					elbow = home_pose_rad[2];					
				}
				sdot= fabs(shoulder-shoulder_sensory)*max_velocity_rad_s* 0.1;
				ddot= fabs(deltoid-deltoid_sensory)*max_velocity_rad_s* 0.1;
				edot= fabs(elbow-elbow_sensory)*max_velocity_rad_s* 0.1;  // 10% of max velocity.
				
				fout << shoulder << ' ' << deltoid<<' ' << elbow << ' ';
				fout << shoulder_sensory << ' ' << deltoid_sensory<<' ' << elbow_sensory << endl;
				
				// DEBUG
				/*
				int row;
				int col = g.cols/2;
				if( t%1000 < 500) row = g.rows-1;
				if( t%1000 >= 500) row = 0;
				*/
					
			
					
					
				/*
				int row = myrand(0,g.rows-1);
				int col = myrand(0,g.cols-1);
				

				shoulder = shoulder_lut_rad[row][col];
				deltoid = deltoid_lut_rad[row][col];
				elbow = elbow_lut_rad[row][col];
				*/
				
				// 2. Move the arm.  
				// It would be nice if the null position was hanging down at its side.
				
				if( sec >= 1 )
				{ 
				  // We don't want to have the arm move to the zero position.
					const int motor_update_interval_ms = 250;
					const int motor_update_offset_ms = 100;
					apex.set_arm_command( shoulder, deltoid, elbow, sdot, ddot, edot,t, motor_update_interval_ms, motor_update_offset_ms);
				}	
			}
		}
	
/*	
		// update DA
		int daFired = 0;
		for (int i=0; i < neurons[ngDA].size(); i++)
			daFired += fired[neurons[ngDA][i]] ? 1 : 0;
		DA = min(4.0, DA+ 2*(double)daFired / (double)neurons[ngDA].size());
		//DA += (double)daFired / (double)neurons[ngDA].size();
		
		dahist << DA << endl;
*/		
	}; // interact()


//**************************************************************************************
// Load group info from file so we know which neurons to read from and stimulate.

	void load_groups(const char fname[])
	{
		cout << "Loading groups ...\n";
		
		max_neuron_id = -1;
		group g;
		fstream gfile(fname);
		while(gfile >> g.area_name)
		{
			gfile >> g.cell_name >> g.rows >> g.cols >> g.first_neuron_id >> g.last_neuron_id;
			cout << g.area_name << ":" <<  g.cell_name << " has " << g.rows*g.cols << " neurons." << endl;
			groups[g.area_name+g.cell_name]=g;
			
			if( g.last_neuron_id > max_neuron_id   )
			{
				max_neuron_id = g.last_neuron_id;
			}
		}

	}; // load_Groups
	
	//******************************
	int get_sim_end_time_sec()
	{
		return sim_end_time_sec;
	};


private:
	
	int sim_end_time_sec;
	
	ofstream fout, patternhist;

  robot apex;
	
	int max_neuron_id;
	
	//vector<float> jointangles;
	map<string, group> groups;

	int count_fired;
	
	int myprocid;
	
	vector<vector<float> > shoulder_lut_rad;
	vector<vector<float> > deltoid_lut_rad;
	vector<vector<float> > elbow_lut_rad;
	
}; // class world 

